<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.11"/>
<title>CL-CBS: include/cl_cbs.hpp Source File</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<link href="search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="search/searchdata.js"></script>
<script type="text/javascript" src="search/search.js"></script>
<script type="text/javascript">
  $(document).ready(function() { init_search(); });
</script>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  <td id="projectalign" style="padding-left: 0.5em;">
   <div id="projectname">CL-CBS
   </div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.8.11 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "search",false,'Search');
</script>
  <div id="navrow1" class="tabs">
    <ul class="tablist">
      <li><a href="index.html"><span>Main&#160;Page</span></a></li>
      <li><a href="namespaces.html"><span>Namespaces</span></a></li>
      <li><a href="annotated.html"><span>Classes</span></a></li>
      <li class="current"><a href="files.html"><span>Files</span></a></li>
      <li><a href="examples.html"><span>Examples</span></a></li>
      <li>
        <div id="MSearchBox" class="MSearchBoxInactive">
        <span class="left">
          <img id="MSearchSelect" src="search/mag_sel.png"
               onmouseover="return searchBox.OnSearchSelectShow()"
               onmouseout="return searchBox.OnSearchSelectHide()"
               alt=""/>
          <input type="text" id="MSearchField" value="Search" accesskey="S"
               onfocus="searchBox.OnSearchFieldFocus(true)" 
               onblur="searchBox.OnSearchFieldFocus(false)" 
               onkeyup="searchBox.OnSearchFieldChange(event)"/>
          </span><span class="right">
            <a id="MSearchClose" href="javascript:searchBox.CloseResultsWindow()"><img id="MSearchCloseImg" border="0" src="search/close.png" alt=""/></a>
          </span>
        </div>
      </li>
    </ul>
  </div>
  <div id="navrow2" class="tabs2">
    <ul class="tablist">
      <li><a href="files.html"><span>File&#160;List</span></a></li>
      <li><a href="globals.html"><span>File&#160;Members</span></a></li>
    </ul>
  </div>
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
</div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0" 
        name="MSearchResults" id="MSearchResults">
</iframe>
</div>

<div id="nav-path" class="navpath">
  <ul>
<li class="navelem"><a class="el" href="dir_d44c64559bbebec7f509842c48db8b23.html">include</a></li>  </ul>
</div>
</div><!-- top -->
<div class="header">
  <div class="headertitle">
<div class="title">cl_cbs.hpp</div>  </div>
</div><!--header-->
<div class="contents">
<a href="cl__cbs_8hpp.html">Go to the documentation of this file.</a><div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno">    1</span>&#160;</div><div class="line"><a name="l00010"></a><span class="lineno">   10</span>&#160;<span class="preprocessor">#pragma once</span></div><div class="line"><a name="l00011"></a><span class="lineno">   11</span>&#160;</div><div class="line"><a name="l00012"></a><span class="lineno">   12</span>&#160;<span class="preprocessor">#include &lt;chrono&gt;</span></div><div class="line"><a name="l00013"></a><span class="lineno">   13</span>&#160;<span class="preprocessor">#include &lt;map&gt;</span></div><div class="line"><a name="l00014"></a><span class="lineno">   14</span>&#160;</div><div class="line"><a name="l00015"></a><span class="lineno">   15</span>&#160;<span class="preprocessor">#include &quot;<a class="code" href="hybrid__astar_8hpp.html">hybrid_astar.hpp</a>&quot;</span></div><div class="line"><a name="l00016"></a><span class="lineno">   16</span>&#160;</div><div class="line"><a name="l00017"></a><span class="lineno"><a class="line" href="cl__cbs_8hpp.html#ae015337362637e20444d4b9e6a033a59">   17</a></span>&#160;<span class="preprocessor">#define MAX_RUNTIME 120</span></div><div class="line"><a name="l00018"></a><span class="lineno">   18</span>&#160;</div><div class="line"><a name="l00019"></a><span class="lineno"><a class="line" href="namespacelibMultiRobotPlanning.html">   19</a></span>&#160;<span class="keyword">namespace </span><a class="code" href="namespacelibMultiRobotPlanning.html">libMultiRobotPlanning</a> {</div><div class="line"><a name="l00086"></a><span class="lineno">   86</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> State, <span class="keyword">typename</span> Action, <span class="keyword">typename</span> Cost, <span class="keyword">typename</span> Conflict,</div><div class="line"><a name="l00087"></a><span class="lineno">   87</span>&#160;          <span class="keyword">typename</span> Constraints, <span class="keyword">typename</span> <a class="code" href="classlibMultiRobotPlanning_1_1Environment.html">Environment</a>&gt;</div><div class="line"><a name="l00088"></a><span class="lineno"><a class="line" href="classlibMultiRobotPlanning_1_1CL__CBS.html">   88</a></span>&#160;<span class="keyword">class </span><a class="code" href="classlibMultiRobotPlanning_1_1CL__CBS.html">CL_CBS</a> {</div><div class="line"><a name="l00089"></a><span class="lineno">   89</span>&#160; <span class="keyword">public</span>:</div><div class="line"><a name="l00090"></a><span class="lineno"><a class="line" href="classlibMultiRobotPlanning_1_1CL__CBS.html#a6e43bc998165cf59e3486b82edfe8ed0">   90</a></span>&#160;  <a class="code" href="classlibMultiRobotPlanning_1_1CL__CBS.html#a6e43bc998165cf59e3486b82edfe8ed0">CL_CBS</a>(<a class="code" href="classlibMultiRobotPlanning_1_1Environment.html">Environment</a>&amp; environment) : m_env(environment) {}</div><div class="line"><a name="l00091"></a><span class="lineno"><a class="line" href="classlibMultiRobotPlanning_1_1CL__CBS.html#a2d262a99dce9ed972cc22f5355fb101a">   91</a></span>&#160;  <a class="code" href="classlibMultiRobotPlanning_1_1CL__CBS.html#a2d262a99dce9ed972cc22f5355fb101a">~CL_CBS</a>() {}</div><div class="line"><a name="l00092"></a><span class="lineno">   92</span>&#160;</div><div class="line"><a name="l00093"></a><span class="lineno"><a class="line" href="classlibMultiRobotPlanning_1_1CL__CBS.html#a0763496725b4f27f8bd680303ca362c9">   93</a></span>&#160;  <span class="keywordtype">bool</span> <a class="code" href="classlibMultiRobotPlanning_1_1CL__CBS.html#a0763496725b4f27f8bd680303ca362c9">search</a>(<span class="keyword">const</span> std::vector&lt;State&gt;&amp; initialStates,</div><div class="line"><a name="l00094"></a><span class="lineno">   94</span>&#160;              std::vector&lt;<a class="code" href="structlibMultiRobotPlanning_1_1PlanResult.html">PlanResult&lt;State, Action, Cost&gt;</a>&gt;&amp; solution) {</div><div class="line"><a name="l00095"></a><span class="lineno">   95</span>&#160;    HighLevelNode start;</div><div class="line"><a name="l00096"></a><span class="lineno">   96</span>&#160;    start.solution.resize(initialStates.size());</div><div class="line"><a name="l00097"></a><span class="lineno">   97</span>&#160;    start.constraints.resize(initialStates.size());</div><div class="line"><a name="l00098"></a><span class="lineno">   98</span>&#160;    start.cost = 0;</div><div class="line"><a name="l00099"></a><span class="lineno">   99</span>&#160;    start.id = 0;</div><div class="line"><a name="l00100"></a><span class="lineno">  100</span>&#160;</div><div class="line"><a name="l00101"></a><span class="lineno">  101</span>&#160;    <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; initialStates.size(); ++i) {</div><div class="line"><a name="l00102"></a><span class="lineno">  102</span>&#160;      <span class="comment">// if (i &lt; solution.size() &amp;&amp; solution[i].states.size() &gt; 1) {</span></div><div class="line"><a name="l00103"></a><span class="lineno">  103</span>&#160;      <span class="comment">//   start.solution[i] = solution[i];</span></div><div class="line"><a name="l00104"></a><span class="lineno">  104</span>&#160;      <span class="comment">//   std::cout &lt;&lt; &quot;use existing solution for agent: &quot; &lt;&lt; i &lt;&lt; std::endl;</span></div><div class="line"><a name="l00105"></a><span class="lineno">  105</span>&#160;      <span class="comment">// } else {</span></div><div class="line"><a name="l00106"></a><span class="lineno">  106</span>&#160;      LowLevelEnvironment llenv(m_env, i, start.constraints[i]);</div><div class="line"><a name="l00107"></a><span class="lineno">  107</span>&#160;      <a class="code" href="classlibMultiRobotPlanning_1_1HybridAStar.html">LowLevelSearch_t</a> lowLevel(llenv);</div><div class="line"><a name="l00108"></a><span class="lineno">  108</span>&#160;      <span class="keywordtype">bool</span> success = lowLevel.<a class="code" href="classlibMultiRobotPlanning_1_1HybridAStar.html#aa25fddfe1d2772051e4d596b728450ec">search</a>(initialStates[i], start.solution[i]);</div><div class="line"><a name="l00109"></a><span class="lineno">  109</span>&#160;      <span class="keywordflow">if</span> (!success) {</div><div class="line"><a name="l00110"></a><span class="lineno">  110</span>&#160;        <span class="keywordflow">return</span> <span class="keyword">false</span>;</div><div class="line"><a name="l00111"></a><span class="lineno">  111</span>&#160;      }</div><div class="line"><a name="l00112"></a><span class="lineno">  112</span>&#160;      <span class="comment">// }</span></div><div class="line"><a name="l00113"></a><span class="lineno">  113</span>&#160;      start.cost += start.solution[i].cost;</div><div class="line"><a name="l00114"></a><span class="lineno">  114</span>&#160;    }</div><div class="line"><a name="l00115"></a><span class="lineno">  115</span>&#160;</div><div class="line"><a name="l00116"></a><span class="lineno">  116</span>&#160;    <span class="comment">// std::priority_queue&lt;HighLevelNode&gt; open;</span></div><div class="line"><a name="l00117"></a><span class="lineno">  117</span>&#160;    <span class="keyword">typename</span> boost::heap::d_ary_heap&lt;HighLevelNode, boost::heap::arity&lt;2&gt;,</div><div class="line"><a name="l00118"></a><span class="lineno">  118</span>&#160;                                     boost::heap::mutable_&lt;true&gt;&gt;</div><div class="line"><a name="l00119"></a><span class="lineno">  119</span>&#160;        open;</div><div class="line"><a name="l00120"></a><span class="lineno">  120</span>&#160;</div><div class="line"><a name="l00121"></a><span class="lineno">  121</span>&#160;    <span class="keyword">auto</span> handle = open.push(start);</div><div class="line"><a name="l00122"></a><span class="lineno">  122</span>&#160;    (*handle).handle = handle;</div><div class="line"><a name="l00123"></a><span class="lineno">  123</span>&#160;</div><div class="line"><a name="l00124"></a><span class="lineno">  124</span>&#160;    std::chrono::high_resolution_clock::time_point</div><div class="line"><a name="l00125"></a><span class="lineno">  125</span>&#160;        startTime = std::chrono::high_resolution_clock::now(),</div><div class="line"><a name="l00126"></a><span class="lineno">  126</span>&#160;        endTime;</div><div class="line"><a name="l00127"></a><span class="lineno">  127</span>&#160;    solution.clear();</div><div class="line"><a name="l00128"></a><span class="lineno">  128</span>&#160;    <span class="keywordtype">int</span> <span class="keywordtype">id</span> = 1;</div><div class="line"><a name="l00129"></a><span class="lineno">  129</span>&#160;    <span class="keywordflow">while</span> (!open.empty()) {</div><div class="line"><a name="l00130"></a><span class="lineno">  130</span>&#160;      endTime = std::chrono::high_resolution_clock::now();</div><div class="line"><a name="l00131"></a><span class="lineno">  131</span>&#160;      <span class="keywordflow">if</span> (std::chrono::duration_cast&lt;std::chrono::duration&lt;double&gt;&gt;(endTime -</div><div class="line"><a name="l00132"></a><span class="lineno">  132</span>&#160;                                                                    startTime)</div><div class="line"><a name="l00133"></a><span class="lineno">  133</span>&#160;              .count() &gt; <a class="code" href="cl__cbs_8hpp.html#ae015337362637e20444d4b9e6a033a59">MAX_RUNTIME</a>) {</div><div class="line"><a name="l00134"></a><span class="lineno">  134</span>&#160;        open.clear();</div><div class="line"><a name="l00135"></a><span class="lineno">  135</span>&#160;        std::cout &lt;&lt; <span class="stringliteral">&quot;\033[1m\033[31m Plan out of runtime time! \033[0m\n&quot;</span>;</div><div class="line"><a name="l00136"></a><span class="lineno">  136</span>&#160;        <span class="keywordflow">return</span> <span class="keyword">false</span>;</div><div class="line"><a name="l00137"></a><span class="lineno">  137</span>&#160;      }</div><div class="line"><a name="l00138"></a><span class="lineno">  138</span>&#160;</div><div class="line"><a name="l00139"></a><span class="lineno">  139</span>&#160;      HighLevelNode P = open.top();</div><div class="line"><a name="l00140"></a><span class="lineno">  140</span>&#160;      m_env.<a class="code" href="classlibMultiRobotPlanning_1_1Environment.html#ae9972a439d882f561c0851a5009eba11">onExpandHighLevelNode</a>(P.cost);</div><div class="line"><a name="l00141"></a><span class="lineno">  141</span>&#160;      <span class="comment">// std::cout &lt;&lt; &quot;expand: &quot; &lt;&lt; P &lt;&lt; std::endl;</span></div><div class="line"><a name="l00142"></a><span class="lineno">  142</span>&#160;</div><div class="line"><a name="l00143"></a><span class="lineno">  143</span>&#160;      open.pop();</div><div class="line"><a name="l00144"></a><span class="lineno">  144</span>&#160;</div><div class="line"><a name="l00145"></a><span class="lineno">  145</span>&#160;      Conflict conflict;</div><div class="line"><a name="l00146"></a><span class="lineno">  146</span>&#160;      <span class="keywordflow">if</span> (!m_env.<a class="code" href="classlibMultiRobotPlanning_1_1Environment.html#a41530a81727bcb3955820112467a4e34">getFirstConflict</a>(P.solution, conflict)) {</div><div class="line"><a name="l00147"></a><span class="lineno">  147</span>&#160;        <span class="comment">// std::cout &lt;&lt; &quot;done; cost: &quot; &lt;&lt; P.cost &lt;&lt; std::endl;</span></div><div class="line"><a name="l00148"></a><span class="lineno">  148</span>&#160;        solution = P.solution;</div><div class="line"><a name="l00149"></a><span class="lineno">  149</span>&#160;        <span class="keywordflow">return</span> <span class="keyword">true</span>;</div><div class="line"><a name="l00150"></a><span class="lineno">  150</span>&#160;      }</div><div class="line"><a name="l00151"></a><span class="lineno">  151</span>&#160;</div><div class="line"><a name="l00152"></a><span class="lineno">  152</span>&#160;      <span class="comment">// create additional nodes to resolve conflict</span></div><div class="line"><a name="l00153"></a><span class="lineno">  153</span>&#160;      <span class="comment">// std::cout &lt;&lt; &quot;Found conflict: &quot; &lt;&lt; conflict &lt;&lt; std::endl;</span></div><div class="line"><a name="l00154"></a><span class="lineno">  154</span>&#160;</div><div class="line"><a name="l00155"></a><span class="lineno">  155</span>&#160;      std::map&lt;size_t, Constraints&gt; constraints;</div><div class="line"><a name="l00156"></a><span class="lineno">  156</span>&#160;      m_env.<a class="code" href="classlibMultiRobotPlanning_1_1Environment.html#a35a9dee492558a6b30f23261bbd9a550">createConstraintsFromConflict</a>(conflict, constraints);</div><div class="line"><a name="l00157"></a><span class="lineno">  157</span>&#160;      <span class="keywordflow">for</span> (<span class="keyword">const</span> <span class="keyword">auto</span>&amp; c : constraints) {</div><div class="line"><a name="l00158"></a><span class="lineno">  158</span>&#160;        <span class="comment">// std::cout &lt;&lt; &quot;Add HL node for &quot; &lt;&lt; c.first &lt;&lt; std::endl;</span></div><div class="line"><a name="l00159"></a><span class="lineno">  159</span>&#160;        <span class="keywordtype">size_t</span> i = c.first;</div><div class="line"><a name="l00160"></a><span class="lineno">  160</span>&#160;        <span class="comment">// std::cout &lt;&lt; &quot;create child with id &quot; &lt;&lt; id &lt;&lt; std::endl;</span></div><div class="line"><a name="l00161"></a><span class="lineno">  161</span>&#160;        HighLevelNode newNode = P;</div><div class="line"><a name="l00162"></a><span class="lineno">  162</span>&#160;        newNode.id = id;</div><div class="line"><a name="l00163"></a><span class="lineno">  163</span>&#160;        <span class="comment">// (optional) check that this constraint was not included already</span></div><div class="line"><a name="l00164"></a><span class="lineno">  164</span>&#160;        <span class="comment">// std::cout &lt;&lt; newNode.constraints[i] &lt;&lt; std::endl;</span></div><div class="line"><a name="l00165"></a><span class="lineno">  165</span>&#160;        <span class="comment">// std::cout &lt;&lt; c.second &lt;&lt; std::endl;</span></div><div class="line"><a name="l00166"></a><span class="lineno">  166</span>&#160;        assert(!newNode.constraints[i].overlap(c.second));</div><div class="line"><a name="l00167"></a><span class="lineno">  167</span>&#160;</div><div class="line"><a name="l00168"></a><span class="lineno">  168</span>&#160;        newNode.constraints[i].add(c.second);</div><div class="line"><a name="l00169"></a><span class="lineno">  169</span>&#160;</div><div class="line"><a name="l00170"></a><span class="lineno">  170</span>&#160;        newNode.cost -= newNode.solution[i].cost;</div><div class="line"><a name="l00171"></a><span class="lineno">  171</span>&#160;</div><div class="line"><a name="l00172"></a><span class="lineno">  172</span>&#160;        LowLevelEnvironment llenv(m_env, i, newNode.constraints[i]);</div><div class="line"><a name="l00173"></a><span class="lineno">  173</span>&#160;        <a class="code" href="classlibMultiRobotPlanning_1_1HybridAStar.html">LowLevelSearch_t</a> lowLevel(llenv);</div><div class="line"><a name="l00174"></a><span class="lineno">  174</span>&#160;        <span class="keywordtype">bool</span> success = lowLevel.<a class="code" href="classlibMultiRobotPlanning_1_1HybridAStar.html#aa25fddfe1d2772051e4d596b728450ec">search</a>(initialStates[i], newNode.solution[i]);</div><div class="line"><a name="l00175"></a><span class="lineno">  175</span>&#160;</div><div class="line"><a name="l00176"></a><span class="lineno">  176</span>&#160;        newNode.cost += newNode.solution[i].cost;</div><div class="line"><a name="l00177"></a><span class="lineno">  177</span>&#160;</div><div class="line"><a name="l00178"></a><span class="lineno">  178</span>&#160;        <span class="keywordflow">if</span> (success) {</div><div class="line"><a name="l00179"></a><span class="lineno">  179</span>&#160;          <span class="comment">// std::cout &lt;&lt; &quot;  success. cost: &quot; &lt;&lt; newNode.cost &lt;&lt; std::endl;</span></div><div class="line"><a name="l00180"></a><span class="lineno">  180</span>&#160;          <span class="keyword">auto</span> handle = open.push(newNode);</div><div class="line"><a name="l00181"></a><span class="lineno">  181</span>&#160;          (*handle).handle = handle;</div><div class="line"><a name="l00182"></a><span class="lineno">  182</span>&#160;        }</div><div class="line"><a name="l00183"></a><span class="lineno">  183</span>&#160;</div><div class="line"><a name="l00184"></a><span class="lineno">  184</span>&#160;        ++id;</div><div class="line"><a name="l00185"></a><span class="lineno">  185</span>&#160;      }</div><div class="line"><a name="l00186"></a><span class="lineno">  186</span>&#160;    }</div><div class="line"><a name="l00187"></a><span class="lineno">  187</span>&#160;</div><div class="line"><a name="l00188"></a><span class="lineno">  188</span>&#160;    <span class="keywordflow">return</span> <span class="keyword">false</span>;</div><div class="line"><a name="l00189"></a><span class="lineno">  189</span>&#160;  }</div><div class="line"><a name="l00190"></a><span class="lineno">  190</span>&#160;</div><div class="line"><a name="l00191"></a><span class="lineno">  191</span>&#160; <span class="keyword">private</span>:</div><div class="line"><a name="l00192"></a><span class="lineno">  192</span>&#160;  <span class="keyword">struct </span>HighLevelNode {</div><div class="line"><a name="l00193"></a><span class="lineno">  193</span>&#160;    std::vector&lt;PlanResult&lt;State, Action, Cost&gt;&gt; solution;</div><div class="line"><a name="l00194"></a><span class="lineno">  194</span>&#160;    std::vector&lt;Constraints&gt; constraints;</div><div class="line"><a name="l00195"></a><span class="lineno">  195</span>&#160;</div><div class="line"><a name="l00196"></a><span class="lineno">  196</span>&#160;    Cost cost;</div><div class="line"><a name="l00197"></a><span class="lineno">  197</span>&#160;</div><div class="line"><a name="l00198"></a><span class="lineno">  198</span>&#160;    <span class="keywordtype">int</span> id;</div><div class="line"><a name="l00199"></a><span class="lineno">  199</span>&#160;</div><div class="line"><a name="l00200"></a><span class="lineno">  200</span>&#160;    <span class="keyword">typename</span> boost::heap::d_ary_heap&lt;HighLevelNode, boost::heap::arity&lt;2&gt;,</div><div class="line"><a name="l00201"></a><span class="lineno">  201</span>&#160;                                     boost::heap::mutable_&lt;true&gt;&gt;::handle_type</div><div class="line"><a name="l00202"></a><span class="lineno">  202</span>&#160;        handle;</div><div class="line"><a name="l00203"></a><span class="lineno">  203</span>&#160;</div><div class="line"><a name="l00204"></a><span class="lineno">  204</span>&#160;    <span class="keywordtype">bool</span> operator&lt;(<span class="keyword">const</span> HighLevelNode&amp; n)<span class="keyword"> const </span>{</div><div class="line"><a name="l00205"></a><span class="lineno">  205</span>&#160;      <span class="comment">// if (cost != n.cost)</span></div><div class="line"><a name="l00206"></a><span class="lineno">  206</span>&#160;      <span class="keywordflow">return</span> cost &gt; n.cost;</div><div class="line"><a name="l00207"></a><span class="lineno">  207</span>&#160;      <span class="comment">// return id &gt; n.id;</span></div><div class="line"><a name="l00208"></a><span class="lineno">  208</span>&#160;    }</div><div class="line"><a name="l00209"></a><span class="lineno">  209</span>&#160;</div><div class="line"><a name="l00210"></a><span class="lineno">  210</span>&#160;    <span class="keyword">friend</span> std::ostream&amp; operator&lt;&lt;(std::ostream&amp; os, <span class="keyword">const</span> HighLevelNode&amp; c) {</div><div class="line"><a name="l00211"></a><span class="lineno">  211</span>&#160;      os &lt;&lt; <span class="stringliteral">&quot;id: &quot;</span> &lt;&lt; c.id &lt;&lt; <span class="stringliteral">&quot; cost: &quot;</span> &lt;&lt; c.cost &lt;&lt; std::endl;</div><div class="line"><a name="l00212"></a><span class="lineno">  212</span>&#160;      <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; c.solution.size(); ++i) {</div><div class="line"><a name="l00213"></a><span class="lineno">  213</span>&#160;        os &lt;&lt; <span class="stringliteral">&quot;Agent: &quot;</span> &lt;&lt; i &lt;&lt; std::endl;</div><div class="line"><a name="l00214"></a><span class="lineno">  214</span>&#160;        os &lt;&lt; <span class="stringliteral">&quot; States:&quot;</span> &lt;&lt; std::endl;</div><div class="line"><a name="l00215"></a><span class="lineno">  215</span>&#160;        <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> t = 0; t &lt; c.solution[i].states.size(); ++t) {</div><div class="line"><a name="l00216"></a><span class="lineno">  216</span>&#160;          os &lt;&lt; <span class="stringliteral">&quot;  &quot;</span> &lt;&lt; c.solution[i].states[t].first &lt;&lt; std::endl;</div><div class="line"><a name="l00217"></a><span class="lineno">  217</span>&#160;        }</div><div class="line"><a name="l00218"></a><span class="lineno">  218</span>&#160;        os &lt;&lt; <span class="stringliteral">&quot; Constraints:&quot;</span> &lt;&lt; std::endl;</div><div class="line"><a name="l00219"></a><span class="lineno">  219</span>&#160;        os &lt;&lt; c.constraints[i];</div><div class="line"><a name="l00220"></a><span class="lineno">  220</span>&#160;        os &lt;&lt; <span class="stringliteral">&quot; cost: &quot;</span> &lt;&lt; c.solution[i].cost &lt;&lt; std::endl;</div><div class="line"><a name="l00221"></a><span class="lineno">  221</span>&#160;      }</div><div class="line"><a name="l00222"></a><span class="lineno">  222</span>&#160;      <span class="keywordflow">return</span> os;</div><div class="line"><a name="l00223"></a><span class="lineno">  223</span>&#160;    }</div><div class="line"><a name="l00224"></a><span class="lineno">  224</span>&#160;  };</div><div class="line"><a name="l00225"></a><span class="lineno">  225</span>&#160;</div><div class="line"><a name="l00226"></a><span class="lineno">  226</span>&#160;  <span class="keyword">struct </span>LowLevelEnvironment {</div><div class="line"><a name="l00227"></a><span class="lineno">  227</span>&#160;    LowLevelEnvironment(<a class="code" href="classlibMultiRobotPlanning_1_1Environment.html">Environment</a>&amp; env, <span class="keywordtype">size_t</span> agentIdx,</div><div class="line"><a name="l00228"></a><span class="lineno">  228</span>&#160;                        <span class="keyword">const</span> Constraints&amp; constraints)</div><div class="line"><a name="l00229"></a><span class="lineno">  229</span>&#160;        : m_env(env)</div><div class="line"><a name="l00230"></a><span class="lineno">  230</span>&#160;    <span class="comment">// , m_agentIdx(agentIdx)</span></div><div class="line"><a name="l00231"></a><span class="lineno">  231</span>&#160;    <span class="comment">// , m_constraints(constraints)</span></div><div class="line"><a name="l00232"></a><span class="lineno">  232</span>&#160;    {</div><div class="line"><a name="l00233"></a><span class="lineno">  233</span>&#160;      m_env.setLowLevelContext(agentIdx, &amp;constraints);</div><div class="line"><a name="l00234"></a><span class="lineno">  234</span>&#160;    }</div><div class="line"><a name="l00235"></a><span class="lineno">  235</span>&#160;</div><div class="line"><a name="l00236"></a><span class="lineno">  236</span>&#160;    Cost admissibleHeuristic(<span class="keyword">const</span> State&amp; s) {</div><div class="line"><a name="l00237"></a><span class="lineno">  237</span>&#160;      <span class="keywordflow">return</span> m_env.admissibleHeuristic(s);</div><div class="line"><a name="l00238"></a><span class="lineno">  238</span>&#160;    }</div><div class="line"><a name="l00239"></a><span class="lineno">  239</span>&#160;</div><div class="line"><a name="l00240"></a><span class="lineno">  240</span>&#160;    <span class="keywordtype">bool</span> isSolution(</div><div class="line"><a name="l00241"></a><span class="lineno">  241</span>&#160;        <span class="keyword">const</span> State&amp; s, Cost g,</div><div class="line"><a name="l00242"></a><span class="lineno">  242</span>&#160;        std::unordered_map&lt;State, std::tuple&lt;State, Action, Cost, Cost&gt;,</div><div class="line"><a name="l00243"></a><span class="lineno">  243</span>&#160;                           std::hash&lt;State&gt;&gt;&amp; camefrom) {</div><div class="line"><a name="l00244"></a><span class="lineno">  244</span>&#160;      <span class="keywordflow">return</span> m_env.isSolution(s, g, camefrom);</div><div class="line"><a name="l00245"></a><span class="lineno">  245</span>&#160;    }</div><div class="line"><a name="l00246"></a><span class="lineno">  246</span>&#160;</div><div class="line"><a name="l00247"></a><span class="lineno">  247</span>&#160;    <span class="keywordtype">void</span> getNeighbors(<span class="keyword">const</span> State&amp; s, Action act,</div><div class="line"><a name="l00248"></a><span class="lineno">  248</span>&#160;                      std::vector&lt;<a class="code" href="structlibMultiRobotPlanning_1_1Neighbor.html">Neighbor&lt;State, Action, Cost&gt;</a>&gt;&amp; neighbors) {</div><div class="line"><a name="l00249"></a><span class="lineno">  249</span>&#160;      m_env.getNeighbors(s, act, neighbors);</div><div class="line"><a name="l00250"></a><span class="lineno">  250</span>&#160;    }</div><div class="line"><a name="l00251"></a><span class="lineno">  251</span>&#160;</div><div class="line"><a name="l00252"></a><span class="lineno">  252</span>&#160;    State getGoal() { <span class="keywordflow">return</span> m_env.getGoal(); }</div><div class="line"><a name="l00253"></a><span class="lineno">  253</span>&#160;</div><div class="line"><a name="l00254"></a><span class="lineno">  254</span>&#160;    <span class="keywordtype">int</span> calcIndex(<span class="keyword">const</span> State&amp; s) { <span class="keywordflow">return</span> m_env.calcIndex(s); }</div><div class="line"><a name="l00255"></a><span class="lineno">  255</span>&#160;</div><div class="line"><a name="l00256"></a><span class="lineno">  256</span>&#160;    <span class="keywordtype">void</span> onExpandNode(<span class="keyword">const</span> State&amp; s, Cost fScore, Cost gScore) {</div><div class="line"><a name="l00257"></a><span class="lineno">  257</span>&#160;      <span class="comment">// std::cout &lt;&lt; &quot;LL expand: &quot; &lt;&lt; s &lt;&lt; std::endl;</span></div><div class="line"><a name="l00258"></a><span class="lineno">  258</span>&#160;      m_env.onExpandLowLevelNode(s, fScore, gScore);</div><div class="line"><a name="l00259"></a><span class="lineno">  259</span>&#160;    }</div><div class="line"><a name="l00260"></a><span class="lineno">  260</span>&#160;</div><div class="line"><a name="l00261"></a><span class="lineno">  261</span>&#160;    <span class="keywordtype">void</span> onDiscover(<span class="keyword">const</span> State&amp; <span class="comment">/*s*/</span>, Cost <span class="comment">/*fScore*/</span>, Cost <span class="comment">/*gScore*/</span>) {</div><div class="line"><a name="l00262"></a><span class="lineno">  262</span>&#160;      <span class="comment">// std::cout &lt;&lt; &quot;LL discover: &quot; &lt;&lt; s &lt;&lt; std::endl;</span></div><div class="line"><a name="l00263"></a><span class="lineno">  263</span>&#160;      <span class="comment">// m_env.onDiscoverLowLevel(s, m_agentIdx, m_constraints);</span></div><div class="line"><a name="l00264"></a><span class="lineno">  264</span>&#160;    }</div><div class="line"><a name="l00265"></a><span class="lineno">  265</span>&#160;</div><div class="line"><a name="l00266"></a><span class="lineno">  266</span>&#160;   <span class="keyword">private</span>:</div><div class="line"><a name="l00267"></a><span class="lineno">  267</span>&#160;    <a class="code" href="classlibMultiRobotPlanning_1_1Environment.html">Environment</a>&amp; m_env;</div><div class="line"><a name="l00268"></a><span class="lineno">  268</span>&#160;  };</div><div class="line"><a name="l00269"></a><span class="lineno">  269</span>&#160;</div><div class="line"><a name="l00270"></a><span class="lineno">  270</span>&#160; <span class="keyword">private</span>:</div><div class="line"><a name="l00271"></a><span class="lineno">  271</span>&#160;  <a class="code" href="classlibMultiRobotPlanning_1_1Environment.html">Environment</a>&amp; m_env;</div><div class="line"><a name="l00272"></a><span class="lineno">  272</span>&#160;  <span class="keyword">typedef</span> <a class="code" href="classlibMultiRobotPlanning_1_1HybridAStar.html">HybridAStar&lt;State, Action, Cost, LowLevelEnvironment&gt;</a></div><div class="line"><a name="l00273"></a><span class="lineno">  273</span>&#160;      <a class="code" href="classlibMultiRobotPlanning_1_1HybridAStar.html">LowLevelSearch_t</a>;</div><div class="line"><a name="l00274"></a><span class="lineno">  274</span>&#160;};</div><div class="line"><a name="l00275"></a><span class="lineno">  275</span>&#160;</div><div class="line"><a name="l00276"></a><span class="lineno">  276</span>&#160;}  <span class="comment">// namespace libMultiRobotPlanning</span></div><div class="ttc" id="classlibMultiRobotPlanning_1_1HybridAStar_html_aa25fddfe1d2772051e4d596b728450ec"><div class="ttname"><a href="classlibMultiRobotPlanning_1_1HybridAStar.html#aa25fddfe1d2772051e4d596b728450ec">libMultiRobotPlanning::HybridAStar::search</a></div><div class="ttdeci">bool search(const State &amp;startState, PlanResult&lt; State, Action, Cost &gt; &amp;solution, Cost initialCost=0)</div><div class="ttdef"><b>Definition:</b> hybrid_astar.hpp:85</div></div>
<div class="ttc" id="hybrid__astar_8hpp_html"><div class="ttname"><a href="hybrid__astar_8hpp.html">hybrid_astar.hpp</a></div><div class="ttdoc">hybrid Astar header </div></div>
<div class="ttc" id="classlibMultiRobotPlanning_1_1Environment_html_ae9972a439d882f561c0851a5009eba11"><div class="ttname"><a href="classlibMultiRobotPlanning_1_1Environment.html#ae9972a439d882f561c0851a5009eba11">libMultiRobotPlanning::Environment::onExpandHighLevelNode</a></div><div class="ttdeci">void onExpandHighLevelNode(int)</div><div class="ttdef"><b>Definition:</b> environment.hpp:172</div></div>
<div class="ttc" id="classlibMultiRobotPlanning_1_1Environment_html_a41530a81727bcb3955820112467a4e34"><div class="ttname"><a href="classlibMultiRobotPlanning_1_1Environment.html#a41530a81727bcb3955820112467a4e34">libMultiRobotPlanning::Environment::getFirstConflict</a></div><div class="ttdeci">bool getFirstConflict(const std::vector&lt; PlanResult&lt; State, Action, double &gt;&gt; &amp;solution, Conflict &amp;result)</div><div class="ttdoc">High Level Environment functions. </div><div class="ttdef"><b>Definition:</b> environment.hpp:133</div></div>
<div class="ttc" id="classlibMultiRobotPlanning_1_1HybridAStar_html"><div class="ttname"><a href="classlibMultiRobotPlanning_1_1HybridAStar.html">libMultiRobotPlanning::HybridAStar</a></div><div class="ttdoc">Spatiotemporal Hybrid A* Algorithm. </div><div class="ttdef"><b>Definition:</b> hybrid_astar.hpp:80</div></div>
<div class="ttc" id="classlibMultiRobotPlanning_1_1CL__CBS_html_a0763496725b4f27f8bd680303ca362c9"><div class="ttname"><a href="classlibMultiRobotPlanning_1_1CL__CBS.html#a0763496725b4f27f8bd680303ca362c9">libMultiRobotPlanning::CL_CBS::search</a></div><div class="ttdeci">bool search(const std::vector&lt; State &gt; &amp;initialStates, std::vector&lt; PlanResult&lt; State, Action, Cost &gt;&gt; &amp;solution)</div><div class="ttdef"><b>Definition:</b> cl_cbs.hpp:93</div></div>
<div class="ttc" id="classlibMultiRobotPlanning_1_1CL__CBS_html_a2d262a99dce9ed972cc22f5355fb101a"><div class="ttname"><a href="classlibMultiRobotPlanning_1_1CL__CBS.html#a2d262a99dce9ed972cc22f5355fb101a">libMultiRobotPlanning::CL_CBS::~CL_CBS</a></div><div class="ttdeci">~CL_CBS()</div><div class="ttdef"><b>Definition:</b> cl_cbs.hpp:91</div></div>
<div class="ttc" id="classlibMultiRobotPlanning_1_1Environment_html"><div class="ttname"><a href="classlibMultiRobotPlanning_1_1Environment.html">libMultiRobotPlanning::Environment</a></div><div class="ttdoc">Environment class. </div><div class="ttdef"><b>Definition:</b> environment.hpp:97</div></div>
<div class="ttc" id="structlibMultiRobotPlanning_1_1Neighbor_html"><div class="ttname"><a href="structlibMultiRobotPlanning_1_1Neighbor.html">libMultiRobotPlanning::Neighbor</a></div><div class="ttdoc">Represents state transations. </div><div class="ttdef"><b>Definition:</b> neighbor.hpp:24</div></div>
<div class="ttc" id="namespacelibMultiRobotPlanning_html"><div class="ttname"><a href="namespacelibMultiRobotPlanning.html">libMultiRobotPlanning</a></div><div class="ttdef"><b>Definition:</b> cl_cbs.hpp:19</div></div>
<div class="ttc" id="classlibMultiRobotPlanning_1_1Environment_html_a35a9dee492558a6b30f23261bbd9a550"><div class="ttname"><a href="classlibMultiRobotPlanning_1_1Environment.html#a35a9dee492558a6b30f23261bbd9a550">libMultiRobotPlanning::Environment::createConstraintsFromConflict</a></div><div class="ttdeci">void createConstraintsFromConflict(const Conflict &amp;conflict, std::map&lt; size_t, Constraints &gt; &amp;constraints)</div><div class="ttdef"><b>Definition:</b> environment.hpp:160</div></div>
<div class="ttc" id="structlibMultiRobotPlanning_1_1PlanResult_html"><div class="ttname"><a href="structlibMultiRobotPlanning_1_1PlanResult.html">libMultiRobotPlanning::PlanResult</a></div><div class="ttdoc">Represents the path for an agent. </div><div class="ttdef"><b>Definition:</b> planresult.hpp:28</div></div>
<div class="ttc" id="classlibMultiRobotPlanning_1_1CL__CBS_html"><div class="ttname"><a href="classlibMultiRobotPlanning_1_1CL__CBS.html">libMultiRobotPlanning::CL_CBS</a></div><div class="ttdoc">Car-Like Conflict-Based Search(CL-CBS) algorithm to solve the Multi-Agent Path Finding for Car-Like r...</div><div class="ttdef"><b>Definition:</b> cl_cbs.hpp:88</div></div>
<div class="ttc" id="classlibMultiRobotPlanning_1_1CL__CBS_html_a6e43bc998165cf59e3486b82edfe8ed0"><div class="ttname"><a href="classlibMultiRobotPlanning_1_1CL__CBS.html#a6e43bc998165cf59e3486b82edfe8ed0">libMultiRobotPlanning::CL_CBS::CL_CBS</a></div><div class="ttdeci">CL_CBS(Environment &amp;environment)</div><div class="ttdef"><b>Definition:</b> cl_cbs.hpp:90</div></div>
<div class="ttc" id="cl__cbs_8hpp_html_ae015337362637e20444d4b9e6a033a59"><div class="ttname"><a href="cl__cbs_8hpp.html#ae015337362637e20444d4b9e6a033a59">MAX_RUNTIME</a></div><div class="ttdeci">#define MAX_RUNTIME</div><div class="ttdef"><b>Definition:</b> cl_cbs.hpp:17</div></div>
</div><!-- fragment --></div><!-- contents -->
<!-- start footer part -->
<hr class="footer"/><address class="footer"><small>
Generated by &#160;<a href="http://www.doxygen.org/index.html">
<img class="footer" src="doxygen.png" alt="doxygen"/>
</a> 1.8.11
</small></address>
</body>
</html>
